#include"esikf_slam/map.h"

namespace esikf_slam{

void Map::insertKeyFrame(Frame::Ptr p_frame){
    p_current_frame_ = p_frame;
    //按道理是新发现的关键帧才会才入的，但是，为了鲁棒性，还是需要进行判断
    if(keyframes_.find(p_frame->keyframe_id_)==keyframes_.end()){
        keyframes_.insert(make_pair(p_frame->keyframe_id_, p_frame));
        active_keyframes_.insert(make_pair(p_frame->keyframe_id_, p_frame));
    } else {  //更相信当前发现的关键帧
        keyframes_[p_frame->keyframe_id_] = p_frame;
        active_keyframes_[p_frame->keyframe_id_] = p_frame;
    }

    if (active_keyframes_.size() > num_active_keyframes_) {
        removeNotGoodKeyframe();
    }else{
        updateLocalmap();
    }
}

void Map::removeNotGoodKeyframe(){
    // 寻找与当前帧最近与最远的两个关键帧
    double max_dis = 0, min_dis = 9999;
    double max_kf_id = 0, min_kf_id = 0;
    auto Twc = p_current_frame_->getPose_world_imu().inverse();
    for (auto& kf : active_keyframes_) {
        if (kf.second == p_current_frame_) continue;  //由于是指针，可以比较指针是否相同
        auto dis = (kf.second->getPose_world_imu() * Twc).log().norm();  //
        if (dis > max_dis) {
            max_dis = dis;
            max_kf_id = kf.first;
        }
        if (dis < min_dis) {
            min_dis = dis;
            min_kf_id = kf.first;
        }
    }

    const double min_dis_th = 0.2;  // 最近阈值
    Frame::Ptr frame_to_remove = nullptr;
    if (min_dis < min_dis_th) {
        // 如果存在很近的帧，优先删掉最近的
        frame_to_remove = keyframes_.at(min_kf_id);
    } else {
        // 删掉最远的
        frame_to_remove = keyframes_.at(max_kf_id);
    }

    ROS_DEBUG_STREAM("remove keyframe " << frame_to_remove->keyframe_id_);
    // remove keyframe 
    active_keyframes_.erase(frame_to_remove->keyframe_id_);
    updateLocalmap();
}

void Map::updateLocalmap(){
    localmap_.reset(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr pc(new pcl::PointCloud<PointType>);
    for(auto& kf : active_keyframes_){
        pcl::transformPointCloud(*kf.second->pc_surf_, *pc, SE3toEigenM(kf.second->getPose_world_imu()));
        *localmap_ = *localmap_ + *pc;
    }
    pcDownSample(localmap_,0.5);
}

pcl::PointCloud<PointType>::Ptr Map::getLocalMap(){
    return localmap_;
}

pcl::PointCloud<PointType>::Ptr Map::getMap(){
    pcl::PointCloud<PointType>::Ptr map(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr pc(new pcl::PointCloud<PointType>);
    for(auto& kf : keyframes_){
        pcl::transformPointCloud(*kf.second->pc_surf_, *pc, SE3toEigenM(kf.second->getPose_world_imu()));
        *map = *map + *pc;
    }
    pcDownSample(map,1);
    return map;
}

}
